#include "sonar.h"
#include "tim.h"
#include "oled.h"
#include "test.h"

#define SONAR_PORT GPIOG
#define TRIG_PORT GPIOG
#define TRIG_PIN GPIO_PIN_1
#define ECHO_PORT GPIOG
#define ECHO_PIN GPIO_PIN_0

#define TIMER htim7
extern TIM_HandleTypeDef TIMER;

static volatile uint32_t start_tick, end_tick;
static volatile uint8_t echo_received;

void HCSR04_Init(void) {
    HAL_TIM_Base_Start(&TIMER);
    //distance_mutex = xSemaphoreCreateMutex();
}

double HCSR04_GetDistance(void)
{
    uint32_t duration = 0;
    float distance = 0;

    // 触发 TRIG 引脚
    HAL_GPIO_WritePin(SONAR_PORT, TRIG_PIN, GPIO_PIN_SET);
    // 延迟 10us
    HAL_Delay(10);
    HAL_GPIO_WritePin(SONAR_PORT, TRIG_PIN, GPIO_PIN_RESET);

    // 等待 ECHO 引脚变高
    while (HAL_GPIO_ReadPin(SONAR_PORT, ECHO_PIN) == GPIO_PIN_RESET);

    // 计时器开始计时
    HAL_TIM_Base_Start(&htim7);

    // 等待 ECHO 引脚变低
    while (HAL_GPIO_ReadPin(SONAR_PORT, ECHO_PIN) == GPIO_PIN_SET);

    // 计时器停止计时
    HAL_TIM_Base_Stop(&htim7);

    // 读取计时器值
    duration = __HAL_TIM_GET_COUNTER(&htim7);

    // 重置计时器值
    __HAL_TIM_SET_COUNTER(&htim7, 0);

    // 计算距离（单位：厘米）
    distance = (double)duration * 4 * 0.0343 / 2.0;

    return distance;
}
